
#include "calculete_seat.hpp"
#include <iostream>
#include <cmath>
void Convert_Seat::set_input_clouds(Angle_Point_Cloud &ap1,Angle_Point_Cloud &ap2)
{
    apc1 = &ap1;  
    apc2 = &ap2;
    std::size_t size1,size2,size0;
    size1 = ap1.my_point.size();
     size2 = ap2.my_point.size();
     if(size1<3)return;
     if(size2<3)return ;
     std::cout<<"start"<<std::endl;
     ppair.clear();
    for(int i=0;i<size1;i++)
        {
            int attribute=ap1.my_point[i].attribute;
            std::size_t size_pair;
            for(int j=0;j<size2;j++)
             {
               if(attribute==ap2.my_point[j].attribute)
               {
                  size0= ppair.size();
                  find_same_len(ap1.my_point[i].p_distance,ap2.my_point[j].p_distance,i,j);
                  // if(size0!=ppair.size()) 
                  // {
                  //   for(int k=size0;k<ppair.size();k++)
                  //       {
                  //       ppair[k].index0=i;
                  //       ppair[k].index2=j;
                  //       std::cout<<i<<"vv "<<j<<std::endl;
                  //       }
                  // }    
               }
             }
        }
    
}
void Convert_Seat::find_same_len(std::vector<std::pair<int, double>> &p_d1,
                  std::vector<std::pair<int, double>> &p_d2,int ki,int kj)
  {
    std::size_t s1,s2;
    Point_Pair pair_point;
    double min_len,tempf;
    int seat;
    s1=p_d1.size();
    s2=p_d2.size();
    if(s1==0)return;
    if(s2==0)return;
    for(int i=0;i<s1;i++)
    {
      min_len=100.0;  
      for(int j=0;j<s1;j++)  
        {
        tempf =p_d1[i].second-p_d2[j].second;
        if(tempf<0.0)tempf=-tempf;
        if(tempf<min_len)
            {
            min_len = tempf;
            seat=j;  
        //     std::cout<<min_len<<" "<<seat<<std::endl;
        //    std::cout<<p_d1[i].second<<" "<<p_d2[j].second<<" "<<tempf<<std::endl;
            }
        }
      if(min_len<LEN_LIMIT)
        {
          pair_point.index0=ki;
         // pair_point.index1=i;
          pair_point.index1 =p_d1[i].first;
          pair_point.index2=kj;
         // pair_point.index3=seat;
         pair_point.index3 =p_d2[seat].first;
          ppair.push_back(pair_point);
          std::cout<<p_d1[i].second<<" "<<p_d2[seat].second<<" "<<min_len<<std::endl;
          std::cout<<ki<<" "<<p_d1[i].first<<" "<<kj<<" "<<p_d2[seat].first<<" "<<std::endl;
         // std::cout<<<<i<<" "<<<<seat<<std::endl;
        }    
    }
  }